/* Include */
#include "../APP.h"

/* --------------------------------- Servo ID map --------------------------------- */
/*

     1      2      3                                           == 7 ==
     4      5      6             
            7   8                                                     == 8 ==
   ===     ===     ===           ------>                              
    ----------------                            == 4 ==        == 5 ==        == 6 == 
       |    L   |                               
       |        |                               == 1 ==        == 2 ==        == 3 ==
       |    R   |
    ----------------
   ===     ===     ===
           17  18
    14     15     16
    11     12     13

*/
/* Define servos' ID*/
#define L1_ID 1
#define L2_ID 2
#define L3_ID 3

#define L4_ID 4
#define L5_ID 5
#define L6_ID 6

#define L7_ID 7
#define L8_ID 8


#define R1_ID 11
#define R2_ID 12
#define R3_ID 13

#define R4_ID 14
#define R5_ID 15
#define R6_ID 16

#define R7_ID 17
#define R8_ID 18


#define T1_ID 21
#define T2_ID 22
#define T3_ID 23
#define T4_ID 24
#define T5_ID 25

/* --------------------------------- Servo ID map --------------------------------- */


/* --------------------------------- Between tasks --------------------------------- */

// JSON_Message JSON_MSG_RX;
struct JSON_Message* p_JSON_MSG_RX_SERVO;

/* --------------------------------- Between tasks --------------------------------- */


/* Function header */
void Task_App_ServoDriver(void* pvParameters);
void Servo_test();
void Servo_setup();
void update_servo_JSON();
void update_servo_value();
void stop_all_servo();


/* App entrance */
void App_ServoDriver()
{
    /* App init */
    HAL_LOG.println("[App_ServoDriver] Initializing");

    /* Create startup thread */
    xTaskCreate(
        Task_App_ServoDriver,
        "App_ServoDriver",
        2048,
        NULL,
        3,
        NULL
    );
}


/* App task */
void Task_App_ServoDriver(void* pvParameters)
{
    (void) pvParameters;

    /* Servo setup */
    Servo_setup();

    /* Loop */
    while (1)
    {
        // Servo_test();
        update_servo_JSON();
        delay(5);
    }
    
    /* End of App_ServoDriver */
    HAL_LOG.println("[App_ServoDriver] Exit");
    vTaskDelete(NULL);
}


/* Servo test */
void Servo_test()
{
    // ServoSCS.RegWritePos(0xfe, 0, 0, 1500);
    // ServoSCS.RegWriteAction();
    ServoSCS.WritePWM(L1_ID, 500);
    // ServoSM.WritePosEx(L7_ID, 4000, 0, 0);
    // ServoSM.WritePosEx(R7_ID, 4000, 0, 0);
    // ServoSM.RegWriteAction();
    delay(5000);

    // ServoSCS.RegWritePos(0xfe, 1023, 0, 1500);
    // ServoSCS.RegWriteAction();
    ServoSCS.WritePWM(L1_ID, -500);
    // ServoSM.WritePosEx(L7_ID, 0, 0, 0);
    // ServoSM.WritePosEx(R7_ID, 0, 0, 0);
    // ServoSM.RegWriteAction();
    delay(5000);
}


/* Servo setup */
void Servo_setup()
{
    /* Set servos to motor mode */
    ServoSCS.PWMMode(L1_ID);
    ServoSCS.PWMMode(L2_ID);
    ServoSCS.PWMMode(L3_ID);
    ServoSCS.PWMMode(R1_ID);
    ServoSCS.PWMMode(R2_ID);
    ServoSCS.PWMMode(R3_ID);

    /* Stop all the wheels */
    stop_all_servo();
}


/* Update servo from JSON msg */
void update_servo_JSON()
{
    if (Queue_2_App_ServoDriver != 0) {
        if ( xQueueReceive(Queue_2_App_ServoDriver, &( p_JSON_MSG_RX_SERVO ),  (TickType_t ) 0) ) {
            /* Update servo value */
            update_servo_value();
        }
    }
}


/* Update servo value */
void update_servo_value()
{
    /* Update wheel value */
    ServoSCS.WritePWM(L1_ID, p_JSON_MSG_RX_SERVO->Servo_L[0]);
    ServoSCS.WritePWM(L2_ID, p_JSON_MSG_RX_SERVO->Servo_L[1]);
    ServoSCS.WritePWM(L3_ID, p_JSON_MSG_RX_SERVO->Servo_L[2]);
    ServoSCS.WritePWM(R1_ID, p_JSON_MSG_RX_SERVO->Servo_R[0]);
    ServoSCS.WritePWM(R2_ID, p_JSON_MSG_RX_SERVO->Servo_R[1]);
    ServoSCS.WritePWM(R3_ID, p_JSON_MSG_RX_SERVO->Servo_R[2]);

    /* Update left small joint */
    ServoSCS.WritePos(L4_ID, p_JSON_MSG_RX_SERVO->Servo_L[3], 0, 0);
    ServoSCS.WritePos(L5_ID, p_JSON_MSG_RX_SERVO->Servo_L[4], 0, 0);
    ServoSCS.WritePos(L6_ID, p_JSON_MSG_RX_SERVO->Servo_L[5], 0, 0);
    ServoSCS.WritePos(L8_ID, p_JSON_MSG_RX_SERVO->Servo_L[7], 0, 0);
    /* Update right small joint */
    ServoSCS.WritePos(R4_ID, p_JSON_MSG_RX_SERVO->Servo_R[3], 0, 0);
    ServoSCS.WritePos(R5_ID, p_JSON_MSG_RX_SERVO->Servo_R[4], 0, 0);
    ServoSCS.WritePos(R6_ID, p_JSON_MSG_RX_SERVO->Servo_R[5], 0, 0);
    ServoSCS.WritePos(R8_ID, p_JSON_MSG_RX_SERVO->Servo_R[7], 0, 0);

    /* Update servo top */
    ServoSCS.WritePos(T1_ID, p_JSON_MSG_RX_SERVO->Servo_T[0], 0, 0);
    ServoSCS.WritePos(T2_ID, p_JSON_MSG_RX_SERVO->Servo_T[1], 0, 0);
    ServoSCS.WritePos(T3_ID, p_JSON_MSG_RX_SERVO->Servo_T[2], 0, 0);
    ServoSCS.WritePos(T4_ID, p_JSON_MSG_RX_SERVO->Servo_T[3], 0, 0);
    ServoSCS.WritePos(T5_ID, p_JSON_MSG_RX_SERVO->Servo_T[4], 0, 0);

    // /* Update big joint */
    ServoSM.WritePosEx(L7_ID, p_JSON_MSG_RX_SERVO->Servo_L[6], 0, 0);
    ServoSM.WritePosEx(R7_ID, p_JSON_MSG_RX_SERVO->Servo_R[6], 0, 0);
}


/* Stop all servos */
void stop_all_servo()
{
    ServoSCS.WritePWM(L1_ID, 0);
    ServoSCS.WritePWM(L2_ID, 0);
    ServoSCS.WritePWM(L3_ID, 0);
    ServoSCS.WritePWM(R1_ID, 0);
    ServoSCS.WritePWM(R2_ID, 0);
    ServoSCS.WritePWM(R3_ID, 0);
}

